#include <stdio.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <string.h>
#include "server.h"
#include "can.h"

// Port 3114 because C = 3rd, A = 1st, N = 14th letter in the alphabet
#define SERVER_PORT 3114
#define CAN_DEVICE "/dev/pcan32"
#define CAN_DEVICE_MAX_LINE 80

// Never edit them
#define NUM_ID_DIGITS 8
#define MAX_HEXSTRLEN NUM_ID_DIGITS
#define MAX_DATA_BYTES 8

// Example Message:
//
// "m e 0x00000123 8  0x11 0x22 0x33 0x44 0x55 0x66 0x77 0x88"
//
//  000000000011111111112222222222333333333344444444445555555 (String- 
//  012345678901234567890123456789012345678901234567890123456  Index)
//
// msg: 0
// fmt: 2
// id : 6 ... 13
// num: 15
// dX : (20 + X*5 ... 20 + X*5 + 1)

int hex2dec(const char* strHex)
{
  int nRetVal = 0;
  char *p;
  
  for(p = (char*)strHex; p < strHex + strlen(strHex); p++)
  {
     if ((p[0] >= '0') && (p[0] <= 'f'))
     {
        nRetVal *= 16;
        if(p[0] < 'A') nRetVal += (p[0] - '0');
        else if(p[0] < 'a') nRetVal += (p[0] - 'A'+10);
        else nRetVal += (p[0] - 'a'+10);
     }
  }
  return nRetVal;
}

int main(int argc, char **argv)
{
   char devstring[CAN_DEVICE_MAX_LINE];
   char hexstring[MAX_HEXSTRLEN+1];
   server_info_t server;
   int devicefd;
   can_t can;
   struct sockaddr_in remotesock;
   socklen_t senderlen;

   int can_read;
   int tcp_read;
   int i,status,established;

   struct timeval timeout;
   fd_set set;

   if (start_server(SERVER_PORT)!=0)
   {
      printf("Error starting the server\n");
      return 1;
   }
   server=get_serverinfo();
   
   /* open device for reading and writing */
   devicefd=open(CAN_DEVICE, O_RDWR | O_NOCTTY);
   if (devicefd<0)
   {
      fprintf(stderr, "Can not Open %s", CAN_DEVICE);
      perror(CAN_DEVICE);
      return (1);
   }

   senderlen=sizeof(struct sockaddr_in);
   while (1)
   {
      printf("Waiting for connection on TCP port %i\n", SERVER_PORT);
      server.clients[0].sockfd=accept(server.sockfd,
                               (struct sockaddr *) &remotesock,
                               &senderlen);
      if (server.clients[0].sockfd <= 0)
      {
         perror("accept");
         return 1;
      }
   
      printf("Accepted connection\n");
      established=1;
   
      do
      {
	 FD_ZERO(&set);
	 FD_SET(server.sockfd, &set);
	 FD_SET(server.clients[0].sockfd, &set);
	 timeout.tv_sec=0;
	 timeout.tv_usec=10000;
         memset(&can,0,sizeof(can_t));

	 status=select(FD_SETSIZE,&set,NULL,NULL,&timeout);
	 if (status<=0)
	 {
	    //printf("TCP timeout\n");
	 }
	 else
	 {
            // Read data from client
	    tcp_read=read(server.clients[0].sockfd,&can,sizeof(can_t));
	    if (tcp_read<0)
	    {
	       perror("read");
	       established=0;
	    }
	    if (tcp_read==0)
	    {
	       established=0;
	    }
	    else
	    {
	       printf("RECV [via TCP]: "); can_dump(&can);

	       if (is_valid(&can))
	       {
		  printf("SEND [via CAN]: "); can_dump(&can);
                  // Build a CAN message to be sent to the Linux PCAN driver
		  sprintf(devstring,
"%c %c 0x%08x %x  0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n",
		     can.msgtype-'A'+'a',
		     can.format-'A'+'a',
		     (unsigned int)ntohl(can.id),
		     can.num,
		     (unsigned char)can.data[0], (unsigned char)can.data[1],
		     (unsigned char)can.data[2], (unsigned char)can.data[3],
		     (unsigned char)can.data[4], (unsigned char)can.data[5],
		     (unsigned char)can.data[6], (unsigned char)can.data[7]);
		  printf("SEND [via DRV]: %s",devstring); 
		  status=write(devicefd,devstring,strlen(devstring));
                  // Send CAN write status back to the client
                  memset(&can,0,sizeof(can_t));
                  can.msgtype='S';
                  can.format='S';
                  can.id=htonl(status);
		  printf("SEND [via TCP]: "); can_dump(&can);
                  writen(server.clients[0].sockfd,&can,sizeof(can_t));
	       }
	       else
	       {
		  printf("*** INVALID ***\n");
		  writen(server.clients[0].sockfd,"Protocol mismatch\n",18);
		  established=0;
	       }
	    }
	 }
	
	 FD_ZERO(&set);
	 FD_SET(devicefd, &set);
	 timeout.tv_sec=0;
	 timeout.tv_usec=10000;
	 status=select(FD_SETSIZE,&set,NULL,NULL,&timeout);
	 if (status<=0)
	 {
	    //printf("CAN timeout\n");
	 }
	 else
	 {
	    can_read=read(devicefd, devstring, CAN_DEVICE_MAX_LINE);
	    if (can_read<0)
	    {
	       perror("read");
	       established=0;
	    }
	    else
	    {
               // Decode a CAN message
	       // Message type
	       memset(hexstring,0,MAX_HEXSTRLEN);
               strncpy(hexstring,devstring+0,1);
	       can.msgtype=hexstring[0]-'a'+'A';
	       // Format
	       strncpy(hexstring,devstring+2,1);
	       can.format=hexstring[0]-'a'+'A';
	       // Identifier
	       strncpy(hexstring,devstring+6,NUM_ID_DIGITS);
	       can.id=htonl(hex2dec(hexstring));
	       // Data length
	       memset(hexstring,0,MAX_HEXSTRLEN);
               strncpy(hexstring,devstring+15,1);
	       can.num=hex2dec(hexstring);
	       for (i=0; i<MAX_DATA_BYTES; i++)
	       {
		  strncpy(hexstring,devstring+20+i*5,2);
		  can.data[i]=hex2dec(hexstring);
	       }
	       printf("RECV [via CAN]: "); can_dump(&can);

	       if (is_valid(&can))
	       {
		  printf("SEND [via TCP]: "); can_dump(&can);
		  writen(server.clients[0].sockfd,&can,sizeof(can_t));
	       }
	       else
	       {
		  printf("OOOH! There is an invalid CAN message.\n");
		  printf("This should never happen, here!\n");
	       }
	    }
	 }	 
      } while (established);
      close(server.clients[0].sockfd);
      printf("Connection closed\n");
   }

   stop_server();
   return 0;
}

